from sympy import Symbol, solve
import numpy
import csv, os
import math

import matplotlib.pyplot as plt

"""
p1 = numpy.array([1, 2, 3])
p2 = numpy.array([4, 6, 9])
p3 = numpy.array([12, 11, 9])

v1 = p3 - p1
v2 = p2 - p1

cp = numpy.cross(v1, v2)

a, b, c = cp

data = []
path = os.getcwd() + '/raw_data.csv'
with open(path, 'r') as myFile:
    lines = csv.reader(myFile)
    for line in lines:
        data.append(line)
"""


def calculate_centre_of_circle(z_index, data=[]):
    z = z_index * 0.05
    points = get_points(z_index, data)

    """
    xx = np.array(xx)
    yy = np.array(yy)
    fig = plt.figure()
    ax = fig.add_subplot(111)
    ax.plot(xx, yy)
    plt.axis("equal")
    plt.show()
    """

    circle_centre_points = []
    for i in range(0, len(points)):
        gp = int(len(points) / 5)
        if i+4*gp >= len(points) or i > 1:
            break
        """
        if i > len(points)/3:
            break
        """
        p = list()
        # A * x*x + B * x*y + C * y*y + D * x + E * y + 1 = 0
        p.append(numpy.array([points[i][0], points[i][1], points[i][2]]))
        p.append(numpy.array([points[i+gp][0], points[i+gp][1], points[i+gp][2]]))
        p.append(numpy.array([points[i+2*gp][0], points[i+2*gp][1], points[i+2*gp][2]]))
        p.append(numpy.array([points[i+3*gp][0], points[i+3*gp][1], points[i+3*gp][2]]))
        p.append(numpy.array([points[i+4*gp][0], points[i+4*gp][1], points[i+4*gp][2]]))

        a = Symbol('a')
        b = Symbol('b')
        c = Symbol('c')
        d = Symbol('d')
        e = Symbol('e')

        equil = list()
        for j in range(0, 5):
            equil.append(a*p[j][0]*p[j][0] + b*p[j][0]*p[j][1] + c*p[j][1]*p[j][1] + d*p[j][0] + e*p[j][1] + 1)
        res = solve(equil, [a, b, c, d, e])

        a = res[a]
        b = res[b]
        c = res[c]
        d = res[d]
        e = res[e]

        x = (b*e - 2*c*d)/(4*a*c - b*b)
        y = (b*d - 2*a*e)/(4*a*c - b*b)
        circle_centre_points.append((x, y, z))

    x = 0
    y = 0
    for item in circle_centre_points:
        x += item[0]
        y += item[1]
    x = x / len(circle_centre_points)
    y = y / len(circle_centre_points)
    z = z

    return [round(x, 2), round(y, 2), round(z, 2)], points


def get_valid_data(datas):
    i = 0
    j = 1000000
    for m in range(10, len(datas)):
        for n in range(0, len(datas[m])):
            if float(datas[m][n]) > -99 and n >= i:
                i = n
                break
        for n in range(len(datas[m]) - 1, -1, -1):
            if float(datas[m][n]) > -99 and n <= j:
                j = n
                break

    new_data = list()
    for m in range(10, len(datas)):
        tmp = list()
        for n in range(0, len(datas[m])):
            if n >= i and n <= j:
                tmp.append(datas[m][n])
        new_data.append(tmp)
    return new_data


def get_points(z_index, data):
    one_set = []
    z = z_index * 0.05
    for d in data:
        one_set.append(d[z_index])

    points = []
    for i in range(0, len(one_set)):
        angle = i * 0.05
        rad = (math.pi / 180) * angle
        x = math.sin(rad) * (300 - float(one_set[i]) + 326.75)
        y = math.cos(rad) * (300 - float(one_set[i]) + 326.75)
        points.append((x, y, z))
    return points


def fix(rawdata):

    c0, point0 = calculate_centre_of_circle(0, rawdata)
    c1, point1 = calculate_centre_of_circle(len(rawdata[0]) - 1, rawdata)

    p0 = numpy.array(c0)
    p1 = numpy.array(c1)

    p00 = numpy.array([0, 0, 0])
    p11 = numpy.array([0, 0, 1])

    P2 = p00 - p11
    P1 = p0 - p1

    dot = numpy.dot(P1, P2)

    cosA = dot / (numpy.linalg.norm(P1) * numpy.linalg.norm(P2))

    D = -P1[0] * c0[0] - P1[1] * c0[1] - P1[2] * c0[2]

    result = list()
    for i in range(0, len(rawdata[0]), 10):
        points = get_points(i, rawdata)
        tmp = []
        for j in range(0, len(points), 2):
            dd = numpy.array(points[j]) - numpy.array(c0)
            dd = numpy.linalg.norm(dd)
            distance = (P1[0] * points[j][0] + P1[1] * points[j][1] + P1[2] * points[j][2] + D) / numpy.linalg.norm(P1)
            r = math.sqrt(dd * dd - distance * distance)
            tmp.append(r)
        result.append(tmp)

    return result



